#pragma once

#include <cv.h>
#include <cxcore.h>
#include <highgui.h>

#include <gsl/gsl_rng.h>
#include <gsl/gsl_randist.h>

#pragma comment (lib, "libgsl.a")

/* number of bins of HSV in histogram */
#define NH 10
#define NS 10
#define NV 10

/* max HSV values */
#define H_MAX 360.0
#define S_MAX 1.0
#define V_MAX 1.0

/* low thresholds on saturation and value for histogramming */
#define S_THRESH 0.1
#define V_THRESH 0.2

/* distribution parameter */
#define LAMBDA 20


typedef struct histogram 
{
    float histo[NH*NS + NV];   /**< histogram array */
    int n;                     /**< length of histogram array */
} histogram;

typedef struct particle {
    float x;          /**< current x coordinate */
    float y;          /**< current y coordinate */
    float s;          /**< scale */
    float xp;         /**< previous x coordinate */
    float yp;         /**< previous y coordinate */
    float sp;         /**< previous scale */
    float x0;         /**< original x coordinate */
    float y0;         /**< original y coordinate */
    int width;        /**< original width of region described by particle */
    int height;       /**< original height of region described by particle */
    histogram* histo; /**< reference histogram describing region being tracked */
    float w;          /**< weight */
} particle;



class CParticleFilter
{
public:
    CParticleFilter(void);
    ~CParticleFilter(void);
public:
    particle* InitParticleFilter(IplImage * pImage,CvRect InitRect);
    CvRect UpdateParticleFilter(particle* &particles,IplImage * pImage);
    void UpdateHist(histogram * &Hist,IplImage * pImage,CvRect r);
    void ReleaseParticleFilter(particle * particles);
    

protected:
    IplImage *pHsv;
    int iImageWidth;
    int iImageHeight;

    int num_particles;

    gsl_rng  *rng;

protected:

    /*This function returns a Gaussian random variate, with mean zero and standard deviation sigma. 
     The probability distribution for Gaussian random variates is,
     for @math{x} in the range @math{-\infty} to @math{+\infty}.
     Use the transformation @math{z = \mu + x} on the numbers 
     returned by gsl_ran_gaussian to obtain a Gaussian distribution with mean @math{\mu}. 
     This function uses the Box-Mueller algorithm which requires two calls the random number generator r. 
     */
    //double gsl_ran_gaussian(gsl_rng * r, double sigma);

     /**
       Converts a BGR image to HSV colorspace

       @param img image to be converted

       @return Returns img converted to a 3-channel, 32-bit HSV image with
         S and V values in the range [0,1] and H value in the range [0,360]
    */
    IplImage* bgr2hsv( IplImage* img );
    /**
       Calculates the histogram bin into which an HSV entry falls
       
       @param h Hue
       @param s Saturation
       @param v Value
       
       @return Returns the bin index corresponding to the HSV color defined by
         \a h, \a s, and \a v.
    */
    int histo_bin( float h, float s, float v );

    /*
  Computes a reference histogram for each of the object regions defined by
  the user

  @param frame video frame in which to compute histograms; should have been
    converted to hsv using bgr2hsv in observation.h
  @param regions regions of \a frame over which histograms should be computed
  @param n number of regions in \a regions
  @param export if TRUE, object region images are exported

  @return Returns an \a n element array of normalized histograms corresponding
    to regions of \a frame specified in \a regions.
*/
    histogram** compute_ref_histos( IplImage* frame, CvRect* regions, int n );
    /**
       Calculates a cumulative histogram as defined above for a given array
       of images
       
       @param imgs an array of images over which to compute a cumulative histogram;
         each must have been converted to HSV colorspace using bgr2hsv()
       @param n the number of images in imgs
       
       @return Returns an un-normalized HSV histogram for \a imgs
    */
    histogram* calc_histogram( IplImage** imgs, int n );

    /**
       Normalizes a histogram so all bins sum to 1.0
       
       @param histo a histogram
    */
    void normalize_histogram( histogram* histo );
    /**
       Computes squared distance metric based on the Battacharyya similarity
       coefficient between histograms.

       @param h1 first histogram; should be normalized
       @param h2 second histogram; should be normalized
       
       @return Rerns a squared distance based on the Battacharyya similarity
         coefficient between \a h1 and \a h2
    */
    float histo_dist_sq( histogram* h1, histogram* h2 );
    /**
       Computes the likelihood of there being a player at a given location in
       an image

       @param img image that has been converted to HSV colorspace using bgr2hsv()
       @param r row location of center of window around which to compute likelihood
       @param c col location of center of window around which to compute likelihood
       @param w width of region over which to compute likelihood
       @param h height of region over which to compute likelihood
       @param ref_histo reference histogram for a player; must have been
         normalized with normalize_histogram()

       @return Returns the likelihood of there being a player at location
         (\a r, \a c) in \a img
    */
    float likelihood( IplImage* img, int r, int c,int w, int h, histogram* ref_histo );
    /**
       Returns an image containing the likelihood of there being a player at
       each pixel location in an image
       
       @param img the image for which likelihood is to be computed; should have
         been converted to HSV colorspace using bgr2hsv()
       @param w width of region over which to compute likelihood
       @param h height of region over which to compute likelihood
       @param ref_histo reference histogram for a player; must have been
         normalized with normalize_histogram()
         
       @return Returns a single-channel, 32-bit floating point image containing
         the likelihood of every pixel location in \a img normalized so that the
         sum of likelihoods is 1.
    */
    IplImage* likelihood_image( IplImage* img, int w, int h, histogram* ref_histo);

     /**
       Creates an initial distribution of particles by sampling from a Gaussian
       window around each of a set of specified locations
       
       
       @param regions an array of regions describing player locations around
         which particles are to be sampled
       @param histos array of histograms describing regions in \a regions
       @param n the number of regions in \a regions
       @param p the total number of particles to be assigned
       
       @return Returns an array of \a p particles sampled from around regions in
         \a regions
    */
    particle* init_distribution(CvRect* regions, histogram** histos, int n, int p);
    /**
       Samples a transition model for a given particle

       @param p a particle to be transitioned
       @param w video frame width
       @param h video frame height
       @param rng a random number generator from which to sample

       @return Returns a new particle sampled based on <EM>p</EM>'s transition
         model
    */
    particle transition( particle p, int w, int h, gsl_rng* rng );

    /**
       Normalizes particle weights so they sum to 1

       @param particles an array of particles whose weights are to be normalized
       @param n the number of particles in \a particles
    */
    void normalize_weights( particle* particles, int n );

    /**
       Re-samples a set of weighted particles to produce a new set of unweighted
       particles

       @param particles an old set of weighted particles whose weights have been
         normalized with normalize_weights()
       @param n the number of particles in \a particles
      
       @return Returns a new set of unweighted particles sampled from \a particles
    */
    particle* resample( particle* particles, int n );
    /**
       Compare two particles based on weight.  For use in qsort.

       @param p1 pointer to a particle
       @param p2 pointer to a particle

       @return Returns -1 if the \a p1 has lower weight than \a p2, 1 if \a p1
         has higher weight than \a p2, and 0 if their weights are equal.
    */
    //int particle_cmp( void* p1, void* p2 );
};
